/*
 * matrixManipulation.cpp : A collection of matrix manipulation fuctions
 *  - matrix addition
 *  - matrix multiplications
 *  - matrix inversions
 *  - minor calculation
 *  - determinant calculation
 */
#include "stdafx.h"
#include "math.h"
#include <iostream>
using namespace std;

/*=================================================================
 * matrixAdd() :
 *      C = A + B
 * Syntax: C = matrixAdd(A,B,l,m)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
double **matrixAdd(double **A, double **B, int l, int m)
{
	double **rMatrix = new double*[l];
	for (int i=0 ; i<l ; i++) {
		rMatrix[i] = new double[m];
		for (int j=0 ; j<m ; j++) {
			rMatrix[i][j] = A[i][j] + B[i][j];
		}
	}
	return rMatrix;
} 

/*=================================================================
 * matrixAddToSelf() A = A + B
 * Syntax: matrixAddToSelf(A,B,l,m)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
void matrixAddToSelf(double **A, double **B, int l, int m)
{
	for (int i=0 ; i<l ; i++) {
		for (int j=0 ; j<m ; j++) {
			A[i][j] += B[i][j];
		}
	}
} 

/*=================================================================
 * matrixSubtract() :
 *      C = A - B
 * Syntax: C = matrixSubtract(A,B,l,m)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
double **matrixSubtract(double **A, double **B, int l, int m)
{
	double **rMatrix = new double*[l];
	for (int i=0 ; i<l ; i++) {
		rMatrix[i] = new double[m];
		for (int j=0 ; j<m ; j++) {
			rMatrix[i][j] = A[i][j] - B[i][j];
		}
	}
	return rMatrix;
} 

/*=================================================================
 * matrixSubtractFromSelf() A = A - B
 * Syntax: matrixSubtractFromSelf(A,B,l,m)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
void matrixSubtractFromSelf(double **A, double **B, int l, int m)
{
	for (int i=0 ; i<l ; i++) {
		for (int j=0 ; j<m ; j++) {
			A[i][j] -= B[i][j];
		}
	}
} 

/*=================================================================
 * matrixMultiplyAB() for 2 dimensional matrices only
 * R (l,n) = A(l,m) * B(m,n)
 * Syntax: R = matrixMultiplyAB(A,B,l,m,n)
 *
 * Dr.Kanok Weerawong
 * 23/04/2010
 *=================================================================
 */
double **matrixMultiplyAB(double **A, double **B, int l, int m, int n)
{
// Memory allocation
	double **rMatrix = new double*[l];
	for (int i = 0; i <l ; i++) {
		rMatrix[i] = new double[n];
	}
// Matrix multiplication
    for (int i = 0; i < l; i++) {
        for (int j = 0; j < n; j++) {
            rMatrix[i][j] = 0.0;
            for (int k = 0; k < m; k++) {
                rMatrix[i][j] += A[i][k] * B[k][j];
            }
        }
    }
	return rMatrix;
}

/*=================================================================
 * matrixMultiplyAtB() for 2 dimensional matrices only
 * R (m,n) = At(m,l) * B(l,n)
 * Syntax: R = matrixMultiplyAtB(A,B,l,m,n)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
double **matrixMultiplyAtB(double **A, double **B, int l, int m, int n)
{
// Memory allocation
	double **rMatrix = new double*[m];
	for (int i = 0; i <m ; i++) {
		rMatrix[i] = new double[n];
	}
// Matrix multiplication
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            rMatrix[i][j] = 0.0;
            for (int k = 0; k < l; k++) {
                rMatrix[i][j] += A[k][i] * B[k][j];
            }
        }
    }
	return rMatrix;
}

/*=================================================================
 * matrixMultiplyABt() for 2 dimensional matrices only
 * R (l,n) = A(l,m) * B(n,m)
 * Syntax: R = matrixMultiplyABt(A,B,l,m,n)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
double **matrixMultiplyABt(double **A, double **B, int l, int m, int n)
{
// Memory allocation
	double **rMatrix = new double*[l];
	for (int i = 0; i <l ; i++) {
		rMatrix[i] = new double[n];
	}
// Matrix multiplication
    for (int i = 0; i < l; i++) {
        for (int j = 0; j < n; j++) {
            rMatrix[i][j] = 0.0;
            for (int k = 0; k < m; k++) {
                rMatrix[i][j] += A[i][k] * B[j][k];
            }
        }
    }
	return rMatrix;
}

/*=================================================================
 * matrixMultiplyAtBA() for 2 dimensional matrices only
 * R (m,m) = At(m,l) * B(l,l) * A(l,m)
 * Syntax: R = matrixMultiplyAtBA(A,B,l,m)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
double **matrixMultiplyAtBA(double **A, double **B, int l, int m)
{
// Memory allocation
	double **rMatrix = new double*[m];
	for (int i = 0; i <m ; i++) {
		rMatrix[i] = new double[m];
	}
	double *dummy = new double[l];

// Matrix multiplication
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < l; j++) {
            dummy[j] = 0.0;
            for (int k = 0; k < l; k++) {
                dummy[j] += A[k][i] * B[k][j];
            }
        }
		for (int j = 0; j < m ; j++){
			rMatrix[i][j] = 0.0;
			for (int k = 0; k < l ; k++){
				rMatrix[i][j] += dummy[k]*A[k][j];
			}
		}
    }
	delete dummy;
	return rMatrix;
}

/*=================================================================
 * matrixMultiplyABAt() for 2 dimensional matrices only
 * R (l,l) = A(l,m) * B(m,m) * At(m,l)
 * Syntax: R = matrixMultiplyAB(A,B,l,m)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
double **matrixMultiplyABAt(double **A, double **B, int l, int m)
{
// Memory allocation
	double **rMatrix = new double*[l];
	for (int i = 0; i <l ; i++) {
		rMatrix[i] = new double[l];
	}
	double *dummy = new double[m];

// Matrix multiplication
    for (int i = 0; i < l; i++) {
        for (int j = 0; j < m; j++) {
            dummy[j] = 0.0;
            for (int k = 0; k < m; k++) {
                dummy[j] += A[i][k] * B[k][j];
            }
        }
		for (int j = 0; j < l ; j++){
			rMatrix[i][j] = 0.0;
			for (int k = 0; k < m ; k++){
				rMatrix[i][j] += dummy[k]*A[j][k];
			}
		}
    }
	delete dummy;
	return rMatrix;
}

/*=================================================================
 * matrixMultiplyABC() for 2 dimensional matrices only
 * R (l,p) = A(l,m) * B(m,n) * C(n,p)
 * Syntax: R = matrixMultiplyABC(A,B,C,l,m,n,p)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
double **matrixMultiplyABC(double **A, double **B, double **C,
							int l, int m, int n, int p)
{
// Memory allocation
	double **rMatrix = new double*[l];
	for (int i = 0; i <l ; i++) {
		rMatrix[i] = new double[p];
	}
	double *dummy = new double[n];

// Matrix multiplication
    for (int i = 0; i < l; i++) {
        for (int j = 0; j < n; j++) {
            dummy[j] = 0.0;
            for (int k = 0; k < m; k++) {
                dummy[j] += A[i][k] * B[k][j];
            }
        }
		for (int j = 0; j < p ; j++){
			rMatrix[i][j] = 0.0;
			for (int k = 0; k < n ; k++){
				rMatrix[i][j] += dummy[k]*C[k][j];
			}
		}
    }
	delete dummy;
	return rMatrix;
}

/*=================================================================
 * matrixMultiplyAtBC() for 2 dimensional matrices only
 * R (l,p) = At(l,m) * B(l,n) * C(n,p)
 * Syntax: R = matrixMultiplyABC(A,B,C,l,m,n,p)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
double **matrixMultiplyAtBC(double **A, double **B, double **C,
							int l, int m, int n, int p)
{
// Memory allocation
	double **rMatrix = new double*[m];
	for (int i = 0; i <m ; i++) {
		rMatrix[i] = new double[p];
	}
	double *dummy = new double[n];

// Matrix multiplication
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            dummy[j] = 0.0;
            for (int k = 0; k < l; k++) {
                dummy[j] += A[k][i] * B[k][j];
            }
        }
		for (int j = 0; j < p ; j++){
			rMatrix[i][j] = 0.0;
			for (int k = 0; k < n ; k++){
				rMatrix[i][j] += dummy[k]*C[k][j];
			}
		}
    }
	delete dummy;
	return rMatrix;
}

/*=================================================================
 * matrixInverseByLu() for 2 dimensional matrices only
 * Syntax: R = matrixInverseByLu(A,n)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
 double **matrixInverseByLu(double **A, int n)
{
// Memory allocation
	double **rMatrix = new double*[n];
	for (int i = 0; i<n ; i++) {
		rMatrix[i] = new double[n];
		for (int j = 0; j<n; j++) {
			rMatrix[i][j] = A[i][j];
		}
	}
	int ks = 0;

    for (int i = 0; i < n; i++) {
		if (rMatrix[i][i] != 0.0){
			ks++;
			for (int j = 0; j < n; j++) {
				if (j != i)
					rMatrix[i][j] = rMatrix[i][j]/rMatrix[i][i];
			}
			rMatrix[i][i] = 1.0/rMatrix[i][i];
		}// end if
		for (int k = 0; k < n; k++) {
			if (k != i) {
				for (int j = 0; j < n ; j++){
					if (j != i)
						rMatrix[k][j] = rMatrix[k][j]-rMatrix[k][i]*rMatrix[i][j];
				}
				rMatrix[k][i] = -rMatrix[k][i]*rMatrix[i][i];
			}// end if
		}
    }
	return rMatrix;
}

/*
 * Minor calculation
 * By Yu-Tseh Jason Chi
 * http://chi3x10.wordpress.com/2008/05/28/calculate-matrix-inversion-in-c
 * 28/05/2008
 */
int matrixGetMinor(double **src, double **dest, int row, int col, int order)
{
    // indicate which col and row is being copied to dest
    int colCount=0,rowCount=0;

    for(int i = 0; i < order; i++ )
    {
        if( i != row )
        {
            colCount = 0;
            for(int j = 0; j < order; j++ )
            {
                // when j is not the element
                if( j != col )
                {
                    dest[rowCount][colCount] = src[i][j];
                    colCount++;
                }
            }
            rowCount++;
        }
    }

    return 1;
}
/*
 * Determinant calculation
 * By Yu-Tseh Jason Chi
 * http://chi3x10.wordpress.com/2008/05/28/calculate-matrix-inversion-in-c
 * 28/05/2008
 */
double matrixGetDeterminant( double **mat, int order)
{
    // order must be >= 0
	// stop the recursion when matrix is a single element
    if( order == 1 )
        return mat[0][0];

    // the determinant value
    double det = 0;

    // allocate the cofactor matrix
    double **minor;
    minor = new double*[order-1];
    for(int i=0;i<order-1;i++)
        minor[i] = new double[order-1];

    for(int i = 0; i < order; i++ )
    {
        // get minor of element (0,i)
        matrixGetMinor( mat, minor, 0, i , order);
        // the recusion is here!
        det += pow( -1.0, i ) * mat[0][i] * matrixGetDeterminant( minor,order-1 );
    }

    // release memory
    for(int i=0;i<order-1;i++)
        delete [] minor[i];
    delete [] minor;

    return det;
}

/*
 * matrix inversion
 * By Yu-Tseh Jason Chi
 * http://chi3x10.wordpress.com/2008/05/28/calculate-matrix-inversion-in-c
 * 28/05/2008
 */
double **matrixInverseByDeterminant(double **A, int order)
{
	double **Y = new double*[order];
	for(int i=0; i<order; i++){
		Y[i] = new double[order];
	}
    // get the determinant of a
    double det = 1.0/matrixGetDeterminant(A,order);

    // memory allocation
    double *temp = new double[(order-1)*(order-1)];
    double **minor = new double*[order-1];
    for(int i=0; i<order-1; i++)
        minor[i] = temp+(i*(order-1));

    for(int j=0; j<order; j++)
    {
        for(int i=0; i<order; i++)
        {
            // get the co-factor (matrix) of A(j,i)
            matrixGetMinor(A,minor,j,i,order);
            Y[i][j] = det*matrixGetDeterminant(minor,order-1);
            if( (i+j)%2 == 1)
                Y[i][j] = -Y[i][j];
        }
    }

    // release memory
    delete [] minor[0];
    delete [] minor;
	return Y;
}
/*=================================================================
 * matrixSetZero() set values of all elements of an existing matrix
 *                 to zero
 * 
 * Syntax: matrixSetZero(A,l,m)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
void matrixSetZero(double **A,int l,int m)
{
	for (int i=0 ; i<l ; i++) {
		for (int j=0 ; j<m ; j++) {
			A[i][j] = 0;
		}
	}
} 

/*=================================================================
 * matrixPrint() for 2 dimensional matrices only
 * 
 * Syntax: matrixPrint(A,l,m)
 *
 * Dr.Kanok Weerawong
 * 25/04/2010
 *=================================================================
 */
void matrixPrint(char *title, double **A,int l,int m)
{
	cout.width(10);
	cout << title ;
	for (int i=0 ; i<l ; i++) {
		cout << endl << "|" ;
		for (int j=0 ; j<m ; j++) {
			cout << "  " << A[i][j] ;
		}
		cout << " |" ;
	}
	cout << "End " << endl;
} 